﻿using System;
using System.Windows;
using System.Windows.Media.Media3D;

namespace LFSRecord2.Misc
{
    public static class MathHelper
    {
        private static Random rand = new Random();
        public static int ProperRandom()
        {
            return rand.Next();
        }
        public static int ProperRandom(int maxValue)
        {
            return rand.Next(maxValue);
        }
        public static int ProperRandom(int minValue, int maxValue)
        {
            return rand.Next(minValue, maxValue);
        }

        private static int _uniqueID = 1;
        public static int UniqueID
        {
            get
            {
                return _uniqueID++;
            }
        }

        public const double RadDegMult = 180 / Math.PI;
        public const double DegRadMult = Math.PI / 180;

        public static double PointDistance(double p1, double p2)
        {
            return Math.Sqrt(Math.Pow((p2 - p1), 2));
        }

        public static double PointDistance(Point p1, Point p2)
        {
            return Math.Sqrt(Math.Pow((p2.X - p1.X), 2) + Math.Pow((p2.Y - p1.Y), 2));
        }

        public static double PointDistance(Point3D p2, Point3D p1)
        {
            return Math.Sqrt(
                Math.Pow((p2.X - p1.X), 2) +
                Math.Pow((p2.Y - p1.Y), 2) +
                Math.Pow((p2.Z - p1.Z), 2)
            );
        }

        public static double DotProduct(Vector vec1, Vector vec2)
        {
            return vec1.X * vec2.X + vec1.Y * vec2.Y;
        }

        public static Point3D Point3DFromHeadingPitch(Point3D pt, double heading, double pitch)
        {
            Point3D pointRotated = RotateX(pitch * Math.PI / 180, pt);
            return RotateZ(heading * Math.PI / 180, pointRotated);
        }

        public static Matrix3D RotateXMatrix3D(double rads)
        {
            // Build a rotation matrix
            Matrix3D mX = new Matrix3D();
            //mX.M11 = 1;
            mX.M22 = Math.Cos(rads);
            mX.M23 = Math.Sin(rads);
            mX.M32 = -Math.Sin(rads);
            mX.M33 = Math.Cos(rads);
            //mX.M44 = 1;
            return mX;
        }

        public static Matrix3D RotateYMatrix3D(double rads)
        {
            // Build a rotation matrix
            Matrix3D mY = new Matrix3D();
            mY.M11 = Math.Cos(rads);
            mY.M13 = -Math.Sin(rads);
            //mY.M22 = 1;
            mY.M31 = Math.Sin(rads);
            mY.M33 = Math.Cos(rads);
            //mY.M44 = 1;
            return mY;
        }

        public static Matrix3D RotateZMatrix3D(double rads)
        {
            // Build a rotation matrix
            Matrix3D mZ = new Matrix3D();
            mZ.M11 = Math.Cos(rads);
            mZ.M12 = Math.Sin(rads);
            mZ.M21 = -Math.Sin(rads);
            mZ.M22 = Math.Cos(rads);
            //mZ.M33 = 1;
            //mZ.M44 = 1;
            return mZ;
        }

        public static Point3D TransformPoint(Point3D pt, double rX, double rY, double rZ, Vector3D vec)
        {
            // Rotation matrix
            Matrix3D m = Matrix3D.Multiply(RotateYMatrix3D(rY), RotateXMatrix3D(rX));
            m = Matrix3D.Multiply(m, RotateZMatrix3D(rZ));

            // Apply Translation
            m.Translate(vec);

            return m.Transform(pt);
        }

        public static Point3D RotatePoint(Point3D pt, double rX, double rY, double rZ)
        {
            Matrix3D m = Matrix3D.Multiply(RotateYMatrix3D(rY), RotateXMatrix3D(rX));
            m = Matrix3D.Multiply(m, RotateZMatrix3D(rZ));

            return m.Transform(pt);
        }

        // aka heading
        public static Point3D RotateZ(double rads, Point3D pt)
        {
            return new Point3D(
                pt.X * Math.Cos(rads) - pt.Y * Math.Sin(rads),
                pt.X * Math.Sin(rads) + pt.Y * Math.Cos(rads),
                pt.Z
            );
        }
        public static Vector3D RotateZ(double rads, Vector3D vec)
        {
            return new Vector3D(
                vec.X * Math.Cos(rads) - vec.Y * Math.Sin(rads),
                vec.X * Math.Sin(rads) + vec.Y * Math.Cos(rads),
                vec.Z
            );
        }

        // aka roll
        public static Point3D RotateY(double rads, Point3D pt)
        {
            return new Point3D(
                pt.Z * Math.Sin(rads) + pt.X * Math.Cos(rads),
                pt.Y,
                pt.Z * Math.Cos(rads) - pt.X * Math.Sin(rads)
                );
        }

        // aka pitch
        public static Point3D RotateX(double rads, Point3D pt)
        {
            return new Point3D(
                pt.X,
                pt.Y * Math.Cos(rads) - pt.Z * Math.Sin(rads),
                pt.Y * Math.Sin(rads) + pt.Z * Math.Cos(rads)
                );
        }
        public static Vector3D RotateX(double rads, Vector3D vec)
        {
            return new Vector3D(
                vec.X,
                vec.Y * Math.Cos(rads) - vec.Z * Math.Sin(rads),
                vec.Y * Math.Sin(rads) + vec.Z * Math.Cos(rads)
                );
        }
    }
}
